#ifndef _VEHICLE_VIZ_H_
#define _VEHICLE_VIZ_H_

#include <dispatch_system/moying_map.h>
#include <dispatch_system/vehicle.h>
#include <dispatch_system/time_based_astar.h>
#include <ros/ros.h>
#include <nav_msgs/OccupancyGrid.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/PolygonStamped.h>
#include <moying_proto/moying_proto.h>

using namespace moying::navigation;

namespace moying
{
namespace visualization
{
    class VehicleViz
    {
        public:
            VehicleViz(const VehiclePtr &vehicle, const MoyingMapPtr &moyingmap, const std::string &global_frame="map");
            virtual ~VehicleViz();

            void publishCellTrajectory(const std::map<int, std::vector<FootprintCell>> &timed_footprints);
            void publishTrajectory(const PlanningResult &result);
            void publishCurrentPose();
            void publishPose(double x, double y, double theta);
            void publishCostmap();
            void publishAStarSearchNodes(const boost::dynamic_bitset<>& nodes,bool closed=true,bool local_map=false,int minx=0,int miny=0,int mint=0);
            void publishRobotOccupancy();
            void publishTimeFootprint(const PlanningResult &result);
            // void publishLocalCostmap(const moying_proto::LocalCostMap& costmap);
            void publishLocalObstacle(const moying_proto::LocalObstacle& obstacle,bool temp=false);
            void publishCollidePolygon(const std::vector<navigation::Coordinate>& coor_vec);
            void publishObstacleOccupancy(const std::vector<std::pair<int,int>>& obstacles);
            void publishLocalCostMap(std::vector<std::vector<unsigned char>> local_cost_map,int min_cx,int min_cy);

        private:
            geometry_msgs::Pose getPoseMsg(double x, double y, double th);
            geometry_msgs::Pose getPoseMsg(const VehicleState &state);
            std_msgs::ColorRGBA getColorMsg();
            ros::NodeHandle nh_;
            VehiclePtr vehicle_;
            std::string global_frame_;
            ros::Publisher traj_pub_;
            ros::Publisher collide_rect_pub_;
            ros::Publisher traj_array_pub_;
            ros::Publisher traj_cells_pub_;
            ros::Publisher pose_pub_;
            ros::Publisher robot_pose_pub_;     //机器人当前costmap占用
            ros::Publisher obs_occupancy_pub_;
            ros::Publisher close_nodes_pub_;    //A star 搜索的闭节点推送
            ros::Publisher time_footprint_pub_;  
            // ros::Publisher local_costmap_pub_;  //局部地图
            ros::Publisher local_obstacle_pub_; //局部障碍物
            ros::Publisher local_temp_obstacle_pub_; //局部临时障碍物
            ros::Publisher local_cost_map_pub_; //局部临时障碍物

            MoyingMapPtr moyingmap_;
    };
    typedef std::shared_ptr<VehicleViz> VehicleVizPtr;
}
}
#endif //_VEHICLE_VIZ_H_